#include "Matrix3D.h"
#include "Point.h"
#include "StdAfx.h"
#include "CGWork.h"
#include <cmath>
#include <iostream>

#include <assert.h>

Matrix3D::Matrix3D(unsigned nRows,unsigned nCols,bool bZeroMatrix)
{
	m_nRows = nRows;
	m_nCols = nCols;
	if(nRows == 0 || nCols == 0)
	{
		m_values = NULL;
		return;
	}
	m_values = new double*[nRows];
	for(unsigned i=0;i<nRows;i++)
	{
		m_values[i] = new double[nCols];
		for(unsigned j=0;j<nCols;j++)
		{
			if(i == j && bZeroMatrix == false)
			{
				m_values[i][j] = 1;
			}
			else
			{
				m_values[i][j] = 0;
			}
			
		}
	}
}

void Matrix3D::CopyMatrix(const Matrix3D& m)
{
	clear();
	m_nRows = m.m_nRows;
	m_nCols = m.m_nCols;
	if(m_nRows == 0 || m_nCols == 0)
	{
		m_values = NULL;
		return;
	}
	m_values = new double*[m_nRows];
	for(unsigned i=0;i<m_nRows;i++)
	{
		m_values[i] = new double[m_nCols];
		for(unsigned j=0;j<m_nCols;j++)
		{
			//row.push_back(0);
			m_values[i][j] = m.m_values[i][j];
		}
	}
}

Matrix3D::Matrix3D(const Matrix3D& m):m_values(NULL)
{
	CopyMatrix(m);
}

const Matrix3D& Matrix3D::operator=(const Matrix3D& m)
{
	CopyMatrix(m);
	return *this;
}

Matrix3D::~Matrix3D()
{
	clear();
}

void Matrix3D::clear()
{
	if(m_values == NULL)
		return;
	for(unsigned i=0;i<m_nRows;i++)
	{
		delete m_values[i];
	}
	delete m_values;
	m_values = NULL;
}

double* Matrix3D::operator[](unsigned unIndex)
{
	if(unIndex >= m_nRows)
		return NULL;
	return m_values[unIndex];
}

const double* Matrix3D::operator[](unsigned unIndex) const
{
	if(unIndex >= m_nRows)
		return NULL;
	return m_values[unIndex];
}

Matrix3D Matrix3D::operator*(const Matrix3D& m) const
{
	Matrix3D ResultMatrix(m_nRows,m.m_nCols,true);
	for(unsigned i=0;i<m_nRows;i++)
	{
		for(unsigned j=0;j<m.m_nCols;j++)
		{
			for(unsigned k=0;k<m.m_nRows;k++)
			{
			    ResultMatrix.m_values[i][j] +=  m_values[i][k] * m.m_values[k][j];
			}
		}
	}
	return ResultMatrix;
}

Matrix3D Matrix3D::operator *(double dbConstant)
{	
	Matrix3D Result(*this);
	Result *= dbConstant;

	return Result;
}

void Matrix3D::operator *=(double dbConstant)
{
	for(unsigned i=0;i<m_nRows;i++)
	{
		for(unsigned j=0;j<m_nCols;j++)
		{
			m_values[i][j] =  m_values[i][j] * dbConstant;
		}
	}
}

Matrix3D Matrix3D::ScaleMatrix(double dbFactor)
{
	cout<<"ScaleMatrix: "<<dbFactor<<endl;
	Matrix3D Scale;
	Scale[0][0]=dbFactor;
	Scale[1][1]=dbFactor;
	Scale[2][2]=dbFactor;
	return Scale;
}

Matrix3D Matrix3D::RotateMatrix(double dbAngle,int nAxis)
{
	Matrix3D Rotate;
	switch (nAxis)
	{
	case ID_AXIS_X:
		Rotate[1][1] = cos(dbAngle);
		Rotate[1][2] = sin(dbAngle);
		Rotate[2][1] = -sin(dbAngle);
		Rotate[2][2] = cos(dbAngle);
		break;
	case ID_AXIS_Y:
		Rotate[0][0] = cos(dbAngle);
		Rotate[0][2] = -sin(dbAngle);
		Rotate[2][0] = sin(dbAngle);
		Rotate[2][2] = cos(dbAngle);
		break;
	case ID_AXIS_Z:
		Rotate[0][0] = cos(dbAngle);
		Rotate[0][1] = sin(dbAngle);
		Rotate[1][0] = -sin(dbAngle);
		Rotate[1][1] = cos(dbAngle);
		break;
	}
	return Rotate;

}

Matrix3D Matrix3D::TranslateMatrix(double dbTx,double dbTy,double dbTz)
{
	Matrix3D Translate;
	Translate[3][0]=dbTx;
	Translate[3][1]=dbTy;
	Translate[3][2]=dbTz;
	return Translate;
}

Matrix3D Matrix3D::TranslateMatrix(double dbT,int nAxis)
{
	switch (nAxis) {
	case ID_AXIS_X:
		return TranslateMatrix(dbT,0,0);
	case ID_AXIS_Y:
		return TranslateMatrix(0,dbT,0);
	case ID_AXIS_Z:
		return TranslateMatrix(0,0,dbT);
	default: // shouldn't happen.
		return TranslateMatrix(0,0,0);
	}
}

Matrix3D Matrix3D::operator-() const
{
	Matrix3D OpMatrix(*this);
	for(unsigned i=0;i<m_nRows;i++)
	{
		for(unsigned j=0;j<m_nCols;j++)
		{
			OpMatrix[i][j] = - OpMatrix[i][j];
		}
	}
	return OpMatrix;
}

Matrix3D Matrix3D::operator-(const Matrix3D &mat) const {
	assert(m_nRows == mat.m_nRows && m_nCols == mat.m_nCols);

	Matrix3D OpMatrix(*this);
	for(unsigned i=0;i<m_nRows;i++)
	{
		for(unsigned j=0;j<m_nCols;j++)
		{
			OpMatrix[i][j] -= mat[i][j];
		}
	}
	return OpMatrix;
}

void Matrix3D::operator+= (const Matrix3D &mat) {
	assert(m_nRows == mat.m_nRows && m_nCols == mat.m_nCols);

	for(unsigned i=0;i<m_nRows;i++)
	{
		for(unsigned j=0;j<m_nCols;j++)
		{
			(*this)[i][j] += mat[i][j];
		}
	}
}

Matrix3D Matrix3D::operator+(const Matrix3D &mat)
{
	assert(m_nRows == mat.m_nRows && m_nCols == mat.m_nCols);
	Matrix3D Result(*this);
	Result +=mat;
	return Result;
}
	
Matrix3D Matrix3D::CrossProduct (const Matrix3D &vector) const {
	assert(m_nRows == vector.m_nRows == 1 && m_nCols > 2 && vector.m_nCols > 2);

	static Matrix3D cross(1,4);
	const Matrix3D &curr(*this);
	cross[0][0] = curr[0][1] * vector[0][2] - curr[0][2] * vector[0][1];
	cross[0][1] = - (curr[0][0] * vector[0][2] - curr[0][2] * vector[0][0]);
	cross[0][2] = curr[0][0] * vector[0][1] - curr[0][1] * vector[0][0];
	cross[0][3] = 1;

	//double size = pow(cross[0][0], 2) + pow(cross[0][1],2) + pow(cross[0][2],2);
	//size = sqrt(size);

	//cross[0][0] /= size;
	//cross[0][1] /= size;
	//cross[0][2] /= size;

	return cross;
}

void Matrix3D::Normalize(double dbToSize)
{
	assert(m_nRows == 1 && m_nCols == 4);

	double dbSum = 
		sqrt( pow(m_values[0][0],2) + pow(m_values[0][1],2) + pow(m_values[0][2],2));
	if(dbSum == 0)
		return;
	for(unsigned i=0;i<3;i++)
	{
		m_values[0][i] /= (dbSum/dbToSize);
	}
	m_values[0][3]=1;
}

int Matrix3D::roundInt(double d)
{
	return static_cast<int>(d+0.5);
}

bool Matrix3D::IsEmpty()
{
	return (m_nCols*m_nRows) == 0;
}